#include "Mesh.h"
#include <algorithm>
#include "Clipper\clipper.hpp"

static float CrossProduct(Vector2 a, Vector2 b)
{
	return a.x*b.y - a.y*b.x;
}

//static bool LinesIntersectHard(Vector2 a, Vector2 b, Vector2 c, Vector2 d)
//{
//	Vector2 r = b - a;
//	Vector2 s = d - c;
//	Vector2 d = c - a;
//	float rxs = CrossProduct(r, s);
//	float t = CrossProduct(d, s) / rxs;
//	float u = CrossProduct(d, r) / rxs;
//	if (rxs == 0) return false;
//
//}

static bool PointInsideAngle(Vector2 a, Vector2 b, Vector2 c, Vector2 d)
{
	Vector2 p1 = b - a;
	Vector2 p2 = c - b;
	Vector2 p3 = d - b;
	float cp1 = CrossProduct(p1, p3);
	float cp2 = CrossProduct(p2, p3);
	float cp3 = CrossProduct(p1, p2);
	if (cp3 > 0) return (cp1 <= 0.0f || cp2 <= 0.0f);
	return (cp1 <= 0.0f && cp2 <= 0.0f);
}

static bool LinesIntersect(Vector2 a, Vector2 b, Vector2 c, Vector2 d)
{
	float denominator = ((b.x - a.x) * (d.y - c.y) - ((b.y - a.y) * (d.x - c.x))); // r x s

	if (denominator == 0)
	{
		return false;
	}

	float numerator1 = ((a.y - c.y) * (d.x - c.x)) - ((a.x - c.x) * (d.y - c.y));

	float numerator2 = ((a.y - c.y) * (b.x - a.x)) - ((a.x - c.x) * (b.y - a.y));

	//if (numerator1 == 0 || numerator2 == 0)
	//{
	//	return false;
	//}

	float r = numerator1 / denominator;
	float s = numerator2 / denominator;

	return (r >= 0.0f && r <= 1.0f) && (s >= 0.0f && s <= 1.0f);
}

static Vector2 LinesIntersection(Vector2 a, Vector2 b, Vector2 c, Vector2 d)
{
	Vector2 r = b - a;
	Vector2 s = d - c;
	Vector2 ca = c - a;
	float rxs = CrossProduct(r, s);
	float t = CrossProduct(ca, s) / rxs;
	return a + t*r;
}

Mesh::Mesh(Poly p)
{
	Border = p;
}

bool Equal(Vector2 a, Vector2 b)
{
	Vector2 p = a - b;
	return (p.x <= 0.001f && p.x >= -0.001f) && (p.y <= 0.001f && p.y >= -0.001f);
}

void Mesh::AddPoly(Poly p)
{
	for (std::vector<Poly>::iterator kt = Holes.begin(); kt != Holes.end();)
	{
		bool f = false;
		PolyVertex* a = p.First(), *cross1 = a, *cross2 = a;
		for (int i = 0; i < p.size; i++)
		{
			PolyVertex* b = kt->First();
			PolyVertex *an = a->next;
			for (int j = 0; j < kt->size; j++)
			{
				PolyVertex *bn = b->next;
				if (LinesIntersect(a->pos, a->next->pos, b->pos, b->next->pos) && a->next != b->next)
				{
					f = true;
					Vector2 cross = LinesIntersection(a->pos, a->next->pos, b->pos, b->next->pos);
					if (b->next->pos == cross)
					{
						cross2 = b->next;
						cross2->prev = a;
					}
					else if (a->next->pos == cross)
					{
						cross1 = a->next;
						cross1->prev = b;
					}
					else if (PointInsideAngle(a->pos, cross, a->next->pos, b->next->pos) || PointInsideAngle(a->next->pos, cross, a->pos, b->pos))
					{
						cross1 = new PolyVertex();
						cross1->pos = cross;
						cross1->prev = b;
						cross1->next = a->next;
						cross1->next->prev = cross1;
					}
					else
					{
						cross2 = new PolyVertex();
						cross2->pos = cross;
						cross2->prev = a;
						cross2->next = b->next;
						cross2->next->prev = cross2;
					}
				}
				b = bn;
			}
			a = an;
		}
		if (!f) ++kt;
		else
		{
			cross1->prev->next = cross1;
			cross2->prev->next = cross2;
			p.first = cross1;
			kt = Holes.erase(kt);
			p.size = p.Size();
		}
	}
	/*for (std::vector<Poly>::iterator kt = Holes.begin(); kt != Holes.end();)
	{
		bool f = false;
		PolyVertex* b = p.First();
		for (int j = 0; j < p.Size(); j++)
		{
			PolyVertex* a = kt->First();
			for (int i = 0; i < kt->Size(); i++)
			{
				if (Equal(a->pos, b->next->pos) && Equal(a->next->pos, b->pos))
				{
					int c = 2;
					PolyVertex *bn = b->next, *an = a->next;
					while (a->prev->pos == bn->next->pos)
					{
						a = a->prev;
						bn = bn->next;
						c -= 2;
					}
					while (b->prev->pos == an->next->pos)
					{
						b = b->prev;
						an = an->next;
						c -= 2;
					}
					p.first = a;
					PolyVertex* tmp = an->next;
					a->next = bn->next;
					a->next->prev = a;
					b->next = tmp;
					b->next->prev = b;
					p.size = kt->Size() + c;
					kt = Holes.erase(kt);
					f = true;
					break;
				}
				a = a->next;
			}
			if (f) break;
			b = b->next;
		}
		if (!f) ++kt;
	}*/
	
	Holes.push_back(p);
}

bool Mesh::LoS(Vector2 a, Vector2 b)
{
	bool los = true;
	for (std::vector<Poly>::iterator kt = Holes.begin(); kt != Holes.end(); ++kt)
	{
		PolyVertex* c = kt->First();
		do
		{
			if (c->pos != a && c->next->pos != a && c->pos != b && c->next->pos != b)
			{
				if (LinesIntersect(a, b, c->pos, c->next->pos))
					los = false;
			}
			c = c->next;
		} while (c != kt->First());
	}
	PolyVertex* c = Border.First();
	do
	{
		if (LinesIntersect(a, b, c->pos, c->next->pos))
			los = false;
		c = c->next;
	} while (c != Border.First());
	return los;
}

void Mesh::UpdateGraph()
{
	NavGraph.Clear();
	if (Holes.empty()) return;
	for (std::vector<Poly>::iterator it = Holes.begin(); it != Holes.end(); ++it)
	{
		PolyVertex* a = it->First();
		do
		{
			if (Border.ContainsPoint(a->pos))
			{
				a->Vertex = NavGraph.AddVertex(a->pos);
				a->hasVertex = true;
			}
			else a->hasVertex = false;
			a = a->next;
		} while (a != it->First());
	}
	/*if (Holes.size() == 1)
	{
		PolyVertex* a = Holes.front().First();
		do
		{
			if (a->hasVertex && a->next->hasVertex)
			{
				a->Vertex->Connect(a->next->Vertex);
			}
				a = a->next;
		} while (a != Holes.front().First());
	}
	else
	{*/
	for (std::vector<Poly>::iterator it = Holes.begin(); it != Holes.end(); ++it)
	{
		PolyVertex* a = it->First();
		do
		{
			if (a->hasVertex)
			{
				if (a->next->hasVertex) a->Vertex->Connect(a->next->Vertex);
				for (std::vector<Poly>::iterator jt = it; jt != Holes.end(); ++jt)
				{
					PolyVertex* b = jt->First();
					do
					{
						if (b->hasVertex)
						{
							if (LoS(a->pos, b->pos) && !PointInsideAngle(a->prev->pos, a->pos, a->next->pos, b->pos) && !PointInsideAngle(b->prev->pos, b->pos, b->next->pos, a->pos))
							{
								a->Vertex->Connect(b->Vertex);
							}
						}
						b = b->next;
					} while (b != jt->First());

				}
			}
			a = a->next;
		} while (a != it->First());
	}
		/*PolyVertex* a = Holes.back().First();
		do
		{
			a->Vertex->Connect(a->next->Vertex);
			a = a->next;
		} while (a != Holes.back().First());*/
	
}

bool Mesh::GetPath(Vector2 start, Vector2 end, std::vector<Vector2>* path)
{
	if (LoS(start, end))
	{
		path->push_back(start);
		path->push_back(end);
		return true;
	}
	Vertex startVertex;
	startVertex.data = start;

	Vertex endVertex;
	endVertex.data = end;

	std::vector<Vertex*> Unexplored = NavGraph.GetVertices();

	std::vector<Vertex*> ToFix;

	for (std::vector<Vertex*>::iterator it = Unexplored.begin(); it != Unexplored.end(); ++it)
	{
		bool losa = LoS(start, (*it)->data);
		bool losb = LoS(end, (*it)->data);
		if (losa)
		{
			startVertex.Connect((*it));
		}
		if (losb)
		{
			endVertex.Connect((*it));
		}
		if (losa || losb) ToFix.push_back(*it);
		if (losa && losb) (*it)->doubleExtra = true;
		else (*it)->doubleExtra = false;
	}

	Unexplored.push_back(&startVertex);
	Unexplored.push_back(&endVertex);
	
	startVertex.value = 0;

	Vertex* c = &startVertex;
	bool success = true;
	while (!Unexplored.empty())
	{
		std::vector<Vertex*>::iterator id = std::min_element(Unexplored.begin(), Unexplored.end(), [](Vertex*a, Vertex* b){ return (a->value >= 0.0f) && (a->value < b->value || b->value < 0.0f); });
		c = *id;
		if (c->value < 0)
		{
			success = false;
			break;
		}
		c->explored = true;
		if (c == &endVertex) 
			break;
		
		for (int i = 0; i < c->Edges.size(); i++)
		{
			Vertex* v = c->Edges[i].Destination;
			if (!v->explored)
			{
				if (v->value > c->Edges[i].Distance + c->value || v->value < 0.0f)
				{
					v->value = c->Edges[i].Distance + c->value;
					v->last = c;
				}
			}
		}
		Unexplored.erase(id);
	}
	if (!path->empty()) path->clear();
	if (success)
	{
		while (c != &startVertex)
		{
			path->push_back(c->data);
			c = c->last;
		}
	}
	path->push_back(startVertex.data);

	std::reverse(path->begin(), path->end());

	for (std::vector<Vertex*>::iterator it = ToFix.begin(); it != ToFix.end(); ++it)
	{
		(*it)->Edges.pop_back();
		if ((*it)->doubleExtra)
		{
			(*it)->Edges.pop_back();
			(*it)->doubleExtra = false;
		}
	}
	return success;
}

Poly::Poly()
{
	size = 0;
}

Poly::Poly(D3DXVECTOR2* points, int count)
{
	size = 0;
	for (int i = 0; i < count; i++)
	{
		Append(points[i]);
	}
}

void Poly::Delete()
{
	PolyVertex *v = first->next;
	while (v != first)
	{
		PolyVertex *c = v->next;
		delete v;
		v = c;
	}
	delete first;
}

//TODO: Check if point is valid
void Poly::Append(D3DXVECTOR2 p)
{
	PolyVertex* v = new PolyVertex();
	v->pos = p;
	if (size == 0)
	{
		first = v;
	}
	else
	{
		v->next = first;
		first->prev = v;
		v->prev = last;
		last->next = v;
	}
	last = v;
	size++;
	if (!valid)
	if (size >= 3)
		valid = true;
}

//void Poly::Remove(int index)
//{
//	if (index == 0)
//	{
//		Vertices.back().next = &Vertices[1];
//		Vertices[1].prev = &Vertices.back();
//	}
//	else if (index == Vertices.size())
//	{
//		Vertices.front().prev = &Vertices[index - 1];
//		Vertices[index - 1].next = &Vertices.front();
//	}
//	else
//	{
//		Vertices[index-1].next = &Vertices[index + 1];
//		Vertices[index + 1].prev = &Vertices[index - 1];
//	}
//	Vertices.erase(Vertices.begin() + index);
//}

//void Poly::Insert(int index, D3DXVECTOR2 v)
//{
//	Vertices.emplace(Vertices.begin() + index);
//	Vertices[index].pos = v;
//	if (index == 0)
//	{
//		Vertices.back().next = &Vertices.front();
//		Vertices.front().prev = &Vertices.back();
//		Vertices[1].prev = &Vertices.front();
//		Vertices.front().next = &Vertices[1];
//	}
//	else if (index == Vertices.size())
//	{
//		Vertices.back().next = &Vertices.front();
//		Vertices.back().prev = &Vertices[index - 1];
//		Vertices[index - 1].next = &Vertices.back();
//		Vertices.front().prev = &Vertices.back();
//	}
//	else
//	{
//		Vertices[index].next = &Vertices[index + 1];
//		Vertices[index].prev = &Vertices[index - 1];
//		Vertices[index - 1].next = &Vertices[index];
//		Vertices[index + 1].prev = &Vertices[index];
//	}
//}

bool Poly::ContainsPoint(Vector2 p)
{
	int nIntersections = 0;
	PolyVertex* v = first;
	do
	{
		if (v->pos.y != v->next->pos.y)
		{
			float sy = v->next->pos.y - v->pos.y;
			float u = (p.y - v->pos.y) / sy;
			if (u >= 0 && u <= 1)
			{
				float sx = v->next->pos.x - v->pos.x;
				if (v->pos.x - p.x + sx * u > 0.0f)
					nIntersections++;
			}
		}
		v = v->next;
	} while (v != first);
	if (nIntersections % 2) return true;
	return false;
}

//Poly Poly::AddBorder(float outset)
//{
//	Poly p;
//	for (std::list<PolyVertex>::iterator c = Vertices.begin(); c != Vertices.end(); ++c)
//	{
//		float l1 = (c->prev->pos.y - c->pos.y) / (c->prev->pos.x - c->pos.x);
//		float l2 = (c->next->pos.y - c->pos.y) / (c->next->pos.x - c->pos.x);
//		float b1 = c->pos.y * c->prev->pos.x - c->pos.x * c->prev->pos.y;
//		float b2 = c->pos.y * c->next->pos.x - c->pos.x * c->next->pos.y;
//		float c1 = sqrt(l1*l1 + 1);
//		float c2 = sqrt(l2*l2 + 1);
//		float d1 = c1*outset + b1;
//		if (c->pos.x > c->prev->pos.x) d1 = -d1;
//		float d2 = c2*outset + b2;
//		if (c->pos.x > c->next->pos.x) d2 = -d2;
//		float x = (d2 - d1) / (l1 - l2);
//		p.Append(D3DXVECTOR2(x, l1*x + d1));
//	}
//	return p;
//}